//These are all the functions that do not require human interaction:

/*---------------------------Claw----------------------------*/

void clawAutoClose() // Controls whether the claw closes when clawLimit is clicked
{
  if (tubeClicked == 1 && clawState != 1 && clawState != 4) // if the tubeLimit is clicked and the claw is not closed or in the process of opening...
  {
    clawState = 2; // ... start closing the claw
  }
  else if (clawState == 2) // if the above condition is not met and the claw is still closing...
  {
    clawState = 3; //... stop the claw
  }
}

void clawAutoCloseA() // closes the claw in autonomous
{
  //if (tubeClicked == 1 && clawState != 1 && clawState != 4)
  //{
    clawState = 2;
  //}
}

// Claw update
void updateClawState() // Updates the claw state variable and moves claws if needed
{
  //Claw State Update
  oldClawState = clawState;
  if (clawState == 4 && SensorValue[OpenLimit] == 1) // if claw is opening and hits open limit
  {
    clawState = 5;
  }
  else if (clawState == 2 && SensorValue[CloseLimit] == 1) // if claw is closing and a tube hits the close limit
  {
    clawState = 1;
  }
  else if ((clawState == 1 && SensorValue[CloseLimit] != 1) || (clawState == 5 && SensorValue[OpenLimit] != 1))
  {
    clawState = 3; //Checks to see if claw is properly opened or closed. If not, sets clawState to 3.
  }
  else if (tubeClicked == 1 && SensorValue[CloseLimit] == 1 && clawState != 4) {
    clawState = 1; // while claw is closed and tube hits the limit switch keep clawState at 1 (constant power)
  }

  // Claw Motor update
  if (clawState == 5) // If claw is completely opened, stop motor
  {
    clawPower = 0;
  }
  else if (clawState == 1) // When claw is auto-closed...
  {
    clawPower = -clawHoldPower; // Continue powering claw to hold tubes
  }
  else if (clawState == 4) // Auto-opens claw
  {
    clawPower = clawOpenPower;
  }
  else if (clawState == 2) // Auto-closes claw
  {
    clawPower = -clawClosePower;
  }
  else if (clawState == 3 && oldClawState == 2)
  {
    clawPower = 0; // If the claw somehow screwed up after close, stop closing
  }
}

/*-----------------------------------Arm-------------------------------------*/
void manageArm() // Changes arm height depending on armState
{
  float kp = 1.4; float ki = 0.01;
  if (armState != 6) {
    int error = armEncode[armState - 1] - SensorValue[ArmEncoder];
    if (abs(error) > armEncodeThreshold)
    {
      if (abs(error) <= 30) {
        int zimp = integral*2/3;
        integral = zimp + error;
      }
      int armOffset = armPower[armState - 1] + error*kp + integral*ki; //PI Controller
      if (armOffset > armUpPower) {
        armOffset = armUpPower;
      }
      else if (armOffset < -armDownPower) {
        armOffset = -armDownPower;
      }
      armAction = 1;
      armLeftPower = armOffset;
      armRightPower = armOffset;
    }
    else {
      armAction = 0;
      armLeftPower = armPower[armState - 1];
      armRightPower = armPower[armState - 1];
    }
  }
}

void calibrateArm() {
  if (SensorValue[in7] > 100 && armTimer < 400) { // Arm goes down until it touches bottom
    armTimer += 1;
    armState = 6;
    armLeftPower = -armDownPower;
    armRightPower = -armDownPower;
  }
  else {
    calibrateArm_inProgress = false;
    armTimer = 0;
	  armLeftPower = 0;
	  armRightPower = 0;
	  armState = 1;
	  SensorValue[ArmEncoder] = 0;
	}
}

// Arm state change
void changeArmState(int upDown)
{
  integral = 0;
  if (upDown == 1)
  {
    if (armState == 4) {
      if (gameState == 2) {
        armState = 4;
      }
      else {
        armState = 3;
      }
    }
    else {
      if (armState == 7 || armState == 8 || armState == 10) {
        armState = 3;
      }
      else if (armState == 9) {
        armState = 4;
      }
      else {
        armState ++;
      }
    }
  }
  else if (upDown == 0)
  {
    if (armState == 1) {
      armState = 1;
    }
    else {
      if (armState == 7 || armState == 8 || armState == 9 || armState == 10)
      {
        armState = 2;
      }
      else {
        armState --;
      }
    }
  }
}

/*---------------------------------Movement--------------------------*/

// Moves robot forward depending on ticks
void moveTilTubeW(int time) { // Moves robot until touches tubes, closes claw and waits until claw is closed
  int timer = 0;
  while (tubeClicked != 1 && timer < time) { // Moves forward while limit switch is not clicked and timer is within time limit
    wheelLeftPower = minMovePower + 12;
    wheelRightPower = minMovePower + 12;
    bob = 3;
    wait1Msec(10);
    timer += 1;
  }
  wheelLeftPower = 0;
  wheelRightPower = 0;
  clawAutoCloseA();
  bob = 5;
  wait1Msec(20);
  timer = 0;
  while (clawState != 1 && clawState!= 3 && timer < 120) {
    wait1Msec(10);
    timer += 1;
  }
}
